#Region ;**** Directives created by AutoIt3Wrapper_GUI ****
#AutoIt3Wrapper_Res_requestedExecutionLevel=asInvoker
#EndRegion ;**** Directives created by AutoIt3Wrapper_GUI ****
#include <GUIConstantsEx.au3>
#include "CommMG.au3"


;==============================================
;==============================================
;SERVER!! Start Me First !!!!!!!!!!!!!!!
;==============================================
;==============================================
Global Const $M_PI =  3.14159265358979323846
Global Const $M_PI_2 =  1.57079632679489661923
Global Const $M_PI_4 =  0.785398163397448309616

Example()


Func Example()
    ; Set Some reusable info
    ; Set your Public IP address (@IPAddress1) here.
;   Local $szServerPC = @ComputerName
;   Local $szIPADDRESS = TCPNameToIP($szServerPC)
    Local $szIPADDRESS = @IPAddress1
	ConsoleWrite($szIPADDRESS)
    Local $nPORT = 33891
    Local $MainSocket, $GOOEY, $edit, $ConnectedSocket, $szIP_Accepted
    Local $msg, $renpcvnp
	Local $recievedString
	Local $scalingFactor, $joyAngle, $joyPosition
	Local $currentTime
	Local $recv
	Local $statusArray
	Local $serverCommFlag
	Local $splitRecv
	Local $dataSplit
	Local $height=400
	Local $width=400
	$error = ""


	;MsgBox(0, "", $ports[0][0])
	;MsgBox(0, "", "Sent message")
    ; Start The TCP Services
    ;==============================================
    TCPStartup()

    ; Create a Listening "SOCKET".
    ;   Using your IP Address and Port 33891.
    ;==============================================
    $MainSocket = TCPListen($szIPADDRESS, $nPORT)

    ; If the Socket creation fails, exit.
    ;If $MainSocket = -1 Then Exit


    ; Create a GUI for messages
    ;==============================================
    $GOOEY = GUICreate("My Server (IP: " & $szIPADDRESS & ")",$width,$height)
	$CmboPortsAvailable = GUICtrlCreateCombo("", 10, $height-25, 145, 25)
	$applyButton = GUICtrlCreateButton("Apply port", 175, $height-27,60,25)
    $edit = GUICtrlCreateEdit("", 10, 10, $width-20, 180)
	$connectedLabel= GUICtrlCreateLabel("Connection Status: ",10,$height-50,90,15)
	$connectedBoxLabel= GuiCtrlCreateLabel("", 110,$height-50, 20,15)
		GUICtrlSetBkColor($connectedBoxLabel, 0xff0000)
		GUICtrlSetColor($connectedBoxLabel, 0x000000)
	$connectCommPortLabel= GUICtrlCreateLabel("No Comm Port Selected", 235, $height-20,150,25)
	$IPLabel = GUICtrlCreateLabel(@IPAddress1, 10, 200, 280, 60, 0x01)
		GUICtrlSetFont($IPLabel, 28, 1600)

	$portlist = _CommListPorts(0);find the available COM ports and write them into the ports combo

	$msg=""

	For $pl = 1 To $portlist[0]
		GUICtrlSetData($CmboPortsAvailable,$portlist[$pl])
	Next


    GUISetState()

	;This runs until you choose a comm port to communicate with.
	;The list of comm ports can be found in the drop down menu,
	;find the comm port you would like to use and click "Apply Port"
	;A verification will be place next to the button if an appropriate
	;Comm Port is selected
	Do
	$msg=GUIGetMsg()
	If $msg == $applyButton Then
	$setport = StringReplace(GUICtrlRead($CmboPortsAvailable),'COM','')
	_CommSetPort($setport, $error, 9600)
	GUICtrlSetData($connectCommPortLabel, "Comm Port " & $setport & " Selected")
	EndIf

	Until $msg ==  $applyButton

    ; Initialize a variable to represent a connection
    ;==============================================
    $ConnectedSocket = -1


    ;Wait for and Accept a connection
    ;==============================================
    Do
        $ConnectedSocket = TCPAccept($MainSocket)
    Until $ConnectedSocket <> -1
	If($connectedSocket<>-1)Then
		GUICtrlSetBkColor($connectedBoxLabel, 0x00ff00)
	EndIf
	TCPSend($ConnectedSocket, "Connected/")
    ; Get IP of client connecting
    $szIP_Accepted = SocketToIP($ConnectedSocket)

    ; GUI Message Loop
    ;==============================================
	$currentTime= TimerInit()
    While 1
		;Added
		If(TimerDiff($currentTime) < 10000)Then
			$currentTime=$currentTime
		Else
			$currentTime= TimerInit()
		EndIf
        $msg = GUIGetMsg()

        ; GUI Closed
        ;--------------------
        If $msg = $GUI_EVENT_CLOSE Then ExitLoop
		If $msg = $applyButton Then
			$setport = StringReplace(GUICtrlRead($CmboPortsAvailable),'COM','')
			_CommSetPort($setport, $error, 9600)
		EndIf
        ; Try to receive (up to) 2048 bytes
        ;----------------------------------------------------------------

		;If anything is recieved from the Basestation,this loops until all information is recieved
		$recv = TCPRecv($ConnectedSocket, 2048)

        ; If the receive failed with @error then the socket has disconnected
        ;----------------------------------------------------------------
        If @error Then
			$ConnectedSocket = -1
			GUICtrlSetBkColor($connectedBoxLabel, 0xff0000)
			Do
				$ConnectedSocket = TCPAccept($MainSocket)
			Until $ConnectedSocket <> -1
		EndIf

		$recievedString= $recv
		While($recv <> "")
			Sleep(10)
			$recv = TCPRecv($ConnectedSocket, 2048)
			$recievedString= $recievedString & $recv
		WEnd
		;If anything is recieved this runs
		If($recievedString<>"")Then
;			ConsoleWrite("Correct " & $recievedString & @CRLF)
			;This parses $recievedString if it is not empty
			;The recievedString should have the format [Flag]\[Data]/[Flag]\[Data]... etc.
			;Make sure to always send [Flag]\ and end the string with "/"
			$splitRecv = StringSplit($recievedString,"/")
			ConsoleWrite("Recieved String is not empty" & @CRLF)
			For $count=1 to $splitRecv[0]-1
;				ConsoleWrite("String Split " &$count & " " &$splitRecv[$count] & @CRLF)
				;This splits the recieved string into its individual components
				;It is split from the "/" character. The returned array is setup so that
				;array[0]= number of elements. Then array[1], array[2], array[3]... Are the individual strings
				;that were recieved
				$flagSplit=StringSplit($splitRecv[$count], "\")
				;If the command "E" was recieved, this sends the exitsafestart() command to the rover
				If($flagSplit[1] =="E") Then
					_CommSendString("E")
				;If M\[Data]/ was recieved then it parses the data
				ElseIf($flagSplit[1] == "M")Then
					$dataSplit= StringSplit($flagSplit[2],"|")
					;dataSplit[1] is the scaling Factor
					If($dataSplit[0] >= 1)Then
						$scalingFactor = $dataSplit[1]
					EndIf
					;dataSplit[2] is the joystick angle
					If($dataSplit[0] >= 2)Then
						$joyAngle = $dataSplit[2]
					EndIf
					;dataSplit[3] is the joystick's postion
					If($dataSplit[0] >= 3)Then
						$joyPosition = $dataSplit[3]
					EndIf
					ConsoleWrite($recievedString)
					ConsoleWrite(@CRLF)
					Sleep(45)
					TankTurning($edit, $scalingFactor, $joyAngle, $joyPosition)
				EndIf
			Next

		EndIf

        ; Update the edit control with what we have received
        ;----------------------------------------------------------------

		;Reads any data from the Arduino and outputs it to the GUI
		$serial = ""
		Sleep(150)
		$serial = _CommGetString()
		If $serial <> "" Then
			GUICtrlSetData($edit,  "Serial > " & $serial & @CRLF & GUICtrlRead($edit))
		EndIf

		If (TimerDiff($currentTime) >= 3000) Then
			;resets the timer
			$currentTime= TimerInit()
			;Tells the arduino to read the status' of the controllers
			getMCStatus($ConnectedSocket)
			$recievedString= ""
			$recv = ""
		EndIf
	WEnd
    If $ConnectedSocket <> -1 Then TCPCloseSocket($ConnectedSocket)

    TCPShutdown()
EndFunc   ;==>Example

; Function to return IP Address from a connected socket.
;----------------------------------------------------------------------
Func SocketToIP($SHOCKET)
    Local $sockaddr, $aRet

    $sockaddr = DllStructCreate("short;ushort;uint;char[8]")

    $aRet = DllCall("Ws2_32.dll", "int", "getpeername", "int", $SHOCKET, _
            "ptr", DllStructGetPtr($sockaddr), "int*", DllStructGetSize($sockaddr))
    If Not @error And $aRet[0] = 0 Then
        $aRet = DllCall("Ws2_32.dll", "str", "inet_ntoa", "int", DllStructGetData($sockaddr, 3))
        If Not @error Then $aRet = $aRet[0]
    Else
        $aRet = 0
    EndIf

    $sockaddr = 0

    Return $aRet
EndFunc   ;==>SocketToIP

;This sends messages to the Arduino in order to set the speeds of each motor
;This requires the GUI ($edit), the scalingFactor (max speed limit), the angle of the joystick
;and the position of the joystick.
Func TankTurning($edit, $scalingFactor, $joystickAngle, $position)
	Local $rightSpeedCommand
	Local $leftSpeedCommand
	Local $degreeFreedom  =(2/180)*$M_PI
	Local $degreeFreedomPI_4 = (1/180)*$M_PI
	;Checks if parameters are appropriate, if not kills motors
	if(($scalingFactor <= 0 or $scalingFactor > 32) or($joystickAngle < 0 or $joystickAngle > (2*$M_PI)) or ($position <= 10 or $position > 100)) Then
		_CommSendString("ML+0000R+0000")
	Else
		;Checks if the joystick is at 45, -45, 135 or -135
		If($joystickAngle >= ($M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=($M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		ElseIf($joystickAngle >= (7*$M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=(7*$M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+0000R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 3*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 3*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML+0000R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 5*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 5*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML-" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		;Checks if the joystick is at 0 or 180 degrees
		ElseIf($joystickAngle > -$degreeFreedom and $joystickAngle < $degreeFreedom) Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < $M_PI+$degreeFreedom and $joystickAngle > $M_PI-$degreeFreedom) Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		;Checks if the joystick is at 90 or -90 degrees
		ElseIf($joystickAngle < $M_PI_2+$degreeFreedom and $joystickAngle > $M_PI_2-$degreeFreedom)Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < 3*$M_PI_2+$degreeFreedom and $joystickAngle > 3*$M_PI_2-$degreeFreedom)Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		Else

			$joystickAngle=($joystickAngle-$M_PI_4)
			;If joystick is between Pi/4 and Pi/2
			If($joystickAngle > $degreeFreedomPI_4 and $joystickAngle < $M_PI_4-$degreeFreedom) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi/2 and 3Pi/4
			ElseIf($joystickAngle > $M_PI_4+$degreeFreedom and $joystickAngle < $M_PI_2-$degreeFreedomPI_4) Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/4 and Pi
			ElseIf($joystickAngle > $M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 3*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-"& String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi and 5Pi/4
			ElseIf($joystickAngle > 3*$M_PI_4+$degreeFreedom and $joystickAngle < $M_PI-$degreeFreedomPI_4)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 5Pi/4 and 3Pi/2
			ElseIf($joystickAngle > $M_PI+$degreeFreedomPI_4 and $joystickAngle < 5*$M_PI_4-$degreeFreedom)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/2 and 7Pi/4
			ElseIf($joystickAngle > 5*$M_PI_4+$degreeFreedom and $joystickAngle < 3*$M_PI_2-$degreeFreedomPI_4)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 7Pi/4 and 2Pi
			ElseIf($joystickAngle > 3*$M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 7*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 0 and Pi/4
			ElseIf($joystickAngle > -$M_PI_4+$degreeFreedom and $joystickAngle < 0-$degreeFreedomPI_4) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))

			EndIf
		EndIf
	EndIf
EndFunc

Func getMCStatus($socketConnected)
	Local $recievedString
	Local $recv
			_CommSendString('S');
			Sleep(100)
			$recv = _CommGetString()
			$recievedString= $recv
				;Loops until all data is sent to the rover from the arduino
				While($recv <> "")
					Sleep(50)
					$recv = _CommGetString()
					$recievedString= $recievedString & $recv
				WEnd
				;Parses the recieved data into information about each motor controller
				If($recievedString <> "")Then
					$statusArray= StringSplit($recievedString,"/")
				;Sends information about the motor controllers, one at a time
				;It doesn't send them in one transmission, just in case something happens
				;during the transmission. Sending them one at a time gives a better chance of
				;information being recieved at the Basestation
					For $sendCount = 1 to $statusArray[0]-1
						TCPSend($socketConnected, "ST" & "\" & $sendCount & $statusArray[$sendCount] & "/")
						ConsoleWrite($statusArray[$sendCount])
						Sleep(2)
					Next
				EndIf
			$recievedString= ""
			$recv = ""
EndFunc
